//******************************************************************************
// mumupi0pi0_h5.cc - A BASF module for reading MDST files and extracting 
// quantities of interest for events consistent with a mu+mu-pi0pi0 final state.
//
// Date: June 2012
// Author: craigb (craig dot bookwalter at pnnl dot gov)
//******************************************************************************

// Standard headers
#include <algorithm>
#include <exception>
#include <iostream>
#include <fstream>
#include <set>
#include <stdexcept>
#include <string>
#include <sstream>
#include <utility>
#include <vector>

// Belle-specific headers
#include "particle/Particle.h"
#include "particle/PID.h"
#include "particle/utility.h"
#include "event/BelleEvent.h"
#include "basf/module.h"
#include "basf/module_descr.h"
#include "benergy/BeamEnergy.h"
#include "kid/atc_pid.h"
#include "ip/IpProfile.h"
#include "helix/Helix.h"


#include "panther/panther.h"
#include MDST_H
#include MDST_OBS_H
#include EVTCLS_H
#include BELLETDF_H

#include "mdst/mdst.h"
#include "mdst/Muid_mdst.h"

// Headers written by me
#include "rows/ypipi_countrow.hpp"
#include "rows/ypipi_mumupi0pi0row.hpp"

#include <pytpp.hpp>
#include <csb-belle.hpp>

#include "ypipi_utils.hpp"

using std::string;
using std::vector;
using std::cout;
using std::cerr;
using std::endl;

static const double RADS_TO_DEG = 360./(2*3.14159265359);
static const double E_MASS = 0.000511;
static const double CHARGED_MASS[5] = {0.000511, 0.105658366, 0.13957018,
				       0.493667, 0.939272046};

#ifdef BELLE_NAMESPACE
namespace Belle {
#endif //BELLE_NAMESPACE

  // All BASF modules inherit from the "Module" class, defined in `basf/module.h'
  // They are written as a single class. The BASF executable loads this class
  // (compiled into a shared object) dynamically. The module constructor, `init' 
  // and `hist_def' methods are all called before reading events from the event 
  // source. The first event is read and BASF calls `begin_run()'. Every time 
  // the experimental run number (contained in the datastream) changes, `end_run()' 
  // will be called and then `begin_run()' again.
  //
  // The `event()' function does the event-by-event processing; in the case of 
  // this module, saving some quantities of interest to an HDF5 dataset for 
  // later study. 
  //
  // When the event source is exhausted, the term() function is called, followed 
  // by the class destructor.
  //----------------------------------------------------------------------------
  class mumupi0pi0_h5: public Module {
  public:
    mumupi0pi0_h5();
    virtual ~mumupi0pi0_h5();
    void init(int *);
    void term();
    void disp_stat(const char*);
    void hist_def();
    void event(BelleEvent*, int*);
    void begin_run(BelleEvent*, int*);
    void end_run(BelleEvent*, int*);

    char outpath_stub[4096];
    char inpath[4096]; 

    static HepPoint3D IP;
    static HepSymMatrix IPerr;

  private:
    float calc_R2();
    float two_track_mass(Mdst_charged& t1, Mdst_charged& t2);

    HepLorentzVector charged_track_p4(Mdst_charged& track, int type);

    bool _write_h5;
    bool _write_hbook;

    int _nevents_tot;
    int _combos_found;

    pyt::table<ypipi_mumupi0pi0row_t>* _nt; 
    ypipi_mumupi0pi0row_t* _row;

    pyt::table<ypipi_countrow_t>* _countnt; 
    ypipi_countrow_t* _countrow;
  };

  HepPoint3D mumupi0pi0_h5::IP = HepPoint3D(0,0,0);
  HepSymMatrix mumupi0pi0_h5::IPerr = HepSymMatrix(3,0);


  //----------------------------------------------------------------------------
  // This function registers the module with the BASF executeable.
  // Any parameters defined in the control script (`mumupi0pi0_h5.basf') are
  // declared and matched with public member variables of the module class.
  //----------------------------------------------------------------------------
  extern "C" Module_descr *mdcl_mumupi0pi0_h5()
  {

    mumupi0pi0_h5 *tmp = new mumupi0pi0_h5();
    Module_descr *dscr = new Module_descr ( "mumupi0pi0_h5", tmp);
    dscr->define_param("outpath_stub", "Path stub to output file", "S", 4096, tmp->outpath_stub);
    dscr->define_param("inpath", "Input file", "S", 4096, tmp->inpath);

    BeamEnergy::define_global(dscr);
    IpProfile::define_global(dscr);

    return dscr;
  }
  
  //----------------------------------------------------------------------------
  // Default constructor
  //----------------------------------------------------------------------------
  mumupi0pi0_h5::mumupi0pi0_h5() : 
  _combos_found(0), 
    _row(new ypipi_mumupi0pi0row_t),
    _countrow(new ypipi_countrow_t),
    _nt(NULL),
    _countnt(NULL)
  {
    // Initialize member vars
  }

  //----------------------------------------------------------------------------
  // Default destructor
  //----------------------------------------------------------------------------  
  mumupi0pi0_h5::~mumupi0pi0_h5() {
    delete _nt;
    delete _row;
    delete _countnt;
    delete _countrow;
  }

  //----------------------------------------------------------------------------
  // In init(), I define one or more output files based on the values of the 
  // parameters passed in from the control scripts.
  //----------------------------------------------------------------------------    
  void mumupi0pi0_h5::init(int* status) {
    
    string path_stub(outpath_stub);


    string outpath = path_stub + ".h5";
      
    _nt = pyt::open_table<ypipi_mumupi0pi0row_t>(outpath, "nt", "w", 
						 "Y(5S) -> mu+mu-pi0pi0 (all combos)",
						 0);
    
    _nt->table_setattr("mdst_path", inpath);
    _nt->table_setattr("nttype", "allcombos");
    _nt->table_setattr("module", "ypipi");
    
    outpath = path_stub + ".count.h5";

    _countnt = pyt::open_table<ypipi_countrow_t>(outpath, "nt", "w", 
						 "Y(5S) -> mu+mu-pi0pi0 (pi0 and ctrk counts)",
						 0);
    
    _countnt->table_setattr("mdst_path", inpath);
    _countnt->table_setattr("module", "ypipi");

    cout << "CRAIGB : Analyzing file " << inpath << endl;
    cout << "CRAIGB : YEAH NINJA!" << endl;

  }
  
  //----------------------------------------------------------------------------
  // hist_def() I leave empty
  //----------------------------------------------------------------------------  
  void mumupi0pi0_h5::hist_def() {
    // Define ntuples here
  }


  //----------------------------------------------------------------------------
  // begin_run() just contains some bookkeeping to make sure the numbers I save
  // are well-calibrated.
  //----------------------------------------------------------------------------    
  void mumupi0pi0_h5::begin_run(BelleEvent* be, int* status) {
    BeamEnergy::begin_run();
    IpProfile::begin_run();
  }
 

  //----------------------------------------------------------------------------
  // h5event() is called if the `h5' output format is requested from the
  // control script; again, it processes a single event and saves quantities of 
  // interest to the output file.
  //----------------------------------------------------------------------------  
  void mumupi0pi0_h5::event(BelleEvent* evptr, int* status) {
    Belle_event_Manager& m_event = Belle_event_Manager::get_manager();

    if (m_event[0].EvtNo() != 3068398)
      return;

    _countrow->expno   = m_event[0].ExpNo();
    _countrow->runno   = m_event[0].RunNo();
    _countrow->eventno = m_event[0].EvtNo();

    _countrow->kept   = 1;
    _countrow->nplus  = -1;
    _countrow->nminus = -1;
    _countrow->ngamma = -1;
    _countrow->npi0   = -1;
    
    Mdst_gamma_Manager& m_gamma     = Mdst_gamma_Manager::get_manager();
    Mdst_charged_Manager& m_charged = Mdst_charged_Manager::get_manager();
    _countrow->ncharged_raw = m_charged.count();
    _countrow->ngamma_raw   = m_gamma.count();

    _countrow->npi0_raw     = -1;
   
    vector<Mdst_gamma> v_gamma;
    std::set<Panther_ID> good_gamma_ids;
    Mdst_ecl_aux_Manager& m_ecl_aux = Mdst_ecl_aux_Manager::get_manager();
    for (Mdst_gamma_Manager::iterator it = m_gamma.begin();
    	 it != m_gamma.end(); it++)
      {
 	if ((it->ecl().match() != 0) && (it->ecl().match() != 2))
 	  continue;
	
  	if (it->ecl().quality() != 0) 
  	  continue;

 	if (it->ecl().energy() < 0.03) 
 	  continue; 

	Mdst_ecl_aux eclaux = m_ecl_aux(it->ecl().get_ID());

 	if (eclaux.e9oe25() < 0.8)
 	  continue;


	v_gamma.push_back(*it);
	good_gamma_ids.insert(it->get_ID());	
      }

    _countrow->ngamma = v_gamma.size();

     if (v_gamma.size() < 3) {
       _countrow->kept = -1;
       _countnt->append(*_countrow);
      
       return;
     }

    //    cout << "CRAIGB : v_gamma.size() = " << v_gamma.size() << endl;

//     if (v_gamma.size() > 200) {
//       _countrow->kept = -1;
//       _countnt->append(*_countrow);

//       return;
//     }


    Mdst_pi0_Manager& m_pi0 = Mdst_pi0_Manager::get_manager();
    _countrow->npi0_raw     = m_pi0.count();

    vector<Mdst_pi0> v_pi0; 

    for (Mdst_pi0_Manager::iterator it = m_pi0.begin(); 
	 it != m_pi0.end(); it++) 
      {
	if (good_gamma_ids.count(it->gamma(0).get_ID()) == 0)
	  continue;

	if (good_gamma_ids.count(it->gamma(1).get_ID()) == 0)
	  continue;
	
	if (fabs(0.135 - it->mass()) >= 0.0054*6) 
	  continue;

	 v_pi0.push_back(*it);
      }

    _countrow->npi0 = v_pi0.size();

    if (v_pi0.size() < 2) {
      _countrow->kept = -2;
      _countnt->append(*_countrow);

      return;
    }

    //    cout << "CRAIGB : v_pi0.size() = " << v_pi0.size() << endl;

    // if (m_pi0.count() > 255) {
    //   _countrow->kept = -3;
    //   _countnt->append(*_countrow);

    //   return;
    // }
    
    if (IpProfile::usable()) {
      IP    = IpProfile::position(1);
      IPerr = IpProfile::position_err_b_life_smeared(1);
    }
    else {
      IP    = HepPoint3D(0,0,0);
      IPerr = HepSymMatrix(3,0);
    }
    
    // Build charged track combos
    vector<Mdst_charged> v_plus;
    vector<Mdst_charged> v_minus;

    for (Mdst_charged_Manager::iterator it = m_charged.begin();
	 it != m_charged.end(); it++) 
      {
	Muid_mdst muid(*it);

	if (!good_charged(*it))
	  continue;

 	if (muid.Muon_likelihood() < 0.1)
 	  continue;

	if (it->charge() > 0)
	  v_plus.push_back(*it);
	else
	  v_minus.push_back(*it);
      }
    
    _countrow->nplus = v_plus.size();
    _countrow->nminus = v_minus.size();
 
    if ((v_plus.size() < 1) || (v_minus.size() < 1)) {
      _countrow->kept = -3;
      _countnt->append(*_countrow);

      return;
    }

    vector<std::pair<Mdst_charged, Mdst_charged> > v_pm;

    for (vector<Mdst_charged>::iterator itp = v_plus.begin(); 
	 itp != v_plus.end();  itp++)
      {
      for (vector<Mdst_charged>::iterator itm = v_minus.begin(); 
	   itm != v_minus.end();  itm++) 
	{
	  if ((two_track_mass(*itp, *itm) < 9))
	    continue;

	  v_pm.push_back(std::pair<Mdst_charged, Mdst_charged>(*itp, *itm));

	}
      }

    _countrow->n_pm_combos = v_pm.size();

    if (v_pm.size() < 1) {
      _countrow->kept = -4;
      _countnt->append(*_countrow);

      return;
    }



    // This mess is equivalent to BeamEnergy::CMBoost()
    double E_BEAM = BeamEnergy::E_beam_corr();
    double E_HER = BeamEnergy::E_HER();
    double E_LER = BeamEnergy::E_LER();
    double cross_angle = BeamEnergy::Cross_angle();

    HepLorentzVector p4_her(0.0,
			    0.0,
			    +sqrt(E_HER*E_HER-E_MASS*E_MASS),
			    E_HER);
    p4_her.rotateY(cross_angle);
    HepLorentzVector p4_ler(0.0,
			    0.0,
			    -sqrt(E_LER*E_LER-E_MASS*E_MASS),
			    E_LER);
    
    HepLorentzVector initial = p4_her + p4_ler; 
    HepLorentzVector initial_cm = p4_her + p4_ler;

    Hep3Vector boost_cm      = initial.boostVector();
    
    initial_cm.boost(-boost_cm);

    vector<std::pair<Mdst_pi0, Mdst_pi0> > v_pi0pi0;

    for (unsigned int i=0; i < v_pi0.size(); i++) 
      for (unsigned int j=i+1; j < v_pi0.size(); j++) {
	HepLorentzVector pi01(v_pi0[i].px(), v_pi0[i].py(), 
			      v_pi0[i].pz(), v_pi0[i].energy());
	HepLorentzVector pi02(v_pi0[j].px(), v_pi0[j].py(), 
			      v_pi0[j].pz(), v_pi0[j].energy());

	double mm_off_pi0pi0 = (initial - pi01 - pi02).m();

	if ((mm_off_pi0pi0 < 9.2) || (mm_off_pi0pi0 > 11))
	    continue;

	v_pi0pi0.push_back(std::pair<Mdst_pi0, Mdst_pi0>(v_pi0[i], v_pi0[j]));
      }


//     cout << "CRAIGB : Event #" << m_event[0].EvtNo() << ", # pi0pi0 = " 
// 	 << v_pi0pi0.size() << endl;

    _countrow->n_pi0pi0_combos = v_pi0pi0.size();
      
    if (v_pi0pi0.size() < 1) {
      _countrow->kept = -5;
      _countnt->append(*_countrow);

      return;
    }


    //    cout << "CRAIGB : v_pi0pi0.size() = " << v_pi0pi0.size() << endl;

    _countnt->append(*_countrow);


    _row->expno   = m_event[0].ExpNo();
    _row->runno   = m_event[0].RunNo();
    _row->eventno = m_event[0].EvtNo();
    _row->beam_e  = BeamEnergy::E_beam_corr();

    atc_pid pid_pi_vs_K(3,1,5,2,3);
    atc_pid pid_mu_vs_K(3,1,5,1,3);
    atc_pid pid_mu_vs_e(3,1,5,1,0);

    std::sort(v_pi0pi0.begin(), v_pi0pi0.end(), pi0pi0_chisq_cmp);

    int best_chi2 = 1;
    for (vector<std::pair<Mdst_charged, Mdst_charged> >::iterator it_ctrk = v_pm.begin();
	 it_ctrk != v_pm.end(); it_ctrk++)
      for (vector<std::pair<Mdst_pi0, Mdst_pi0> >::iterator it_pi0 = v_pi0pi0.begin();
	   it_pi0 != v_pi0pi0.end(); it_pi0++)
	{
	  // _row->pm_combono = it_ctrk - v_pm.begin();
	  // _row->pi0pi0_combono = it_pi0 - v_pi0pi0.begin();

	  _row->best_chi2 = best_chi2;
	  best_chi2 = 0;

	  Muid_mdst muid_mup(it_ctrk->first);
	  Muid_mdst muid_mum(it_ctrk->second);
	  
	  _row->mup_muid_llh             = muid_mup.Muon_likelihood();
	  _row->mup_pion_vs_kaon_llh     = pid_pi_vs_K.prob(it_ctrk->first);
	  _row->mup_muon_vs_kaon_llh     = pid_mu_vs_K.prob(it_ctrk->first);
	  _row->mup_muon_vs_electron_llh = pid_mu_vs_e.prob(it_ctrk->first);

	  _row->mum_muid_llh             = muid_mum.Muon_likelihood();
	  _row->mum_pion_vs_kaon_llh     = pid_pi_vs_K.prob(it_ctrk->second);
	  _row->mum_muon_vs_kaon_llh     = pid_mu_vs_K.prob(it_ctrk->second);
	  _row->mum_muon_vs_electron_llh = pid_mu_vs_e.prob(it_ctrk->second);

	  HepLorentzVector mup = charged_track_p4(it_ctrk->first, 1);
	  HepLorentzVector mum = charged_track_p4(it_ctrk->second, 1);
	 
	  _row->mup_px = mup.px();
	  _row->mup_py = mup.py();
	  _row->mup_pz = mup.pz();
	  _row->mup_e  = mup.e();
	  
	  _row->mum_px = mum.px();
	  _row->mum_py = mum.py();
	  _row->mum_pz = mum.pz();
	  _row->mum_e  = mum.e();
	  
	  _row->r2 = calc_R2();
    
	  _row->m_mumu = (mup + mum).m();
	  _row->mm_off_mumu = (initial - mup - mum).m();
    
	  Mdst_pi0 pi01_mdst = it_pi0->first;
	  Mdst_pi0 pi02_mdst = it_pi0->second;
	  
	  if (it_pi0->first.energy() < it_pi0->second.energy()) {
	    pi01_mdst = it_pi0->second;
	    pi02_mdst = it_pi0->first;
	  }
	  
	  _row->mumupi0pi0_idhash = mumupi0pi0_idhash(it_ctrk->first.get_ID(), 
						      it_ctrk->second.get_ID(),
						      pi01_mdst.get_ID(),
						      pi02_mdst.get_ID());

	  std::set<Panther_ID> gamma_IDs;
	  
	  gamma_IDs.insert(pi01_mdst.gamma(0).get_ID());
	  gamma_IDs.insert(pi01_mdst.gamma(1).get_ID());
	  gamma_IDs.insert(pi02_mdst.gamma(0).get_ID());
	  gamma_IDs.insert(pi02_mdst.gamma(1).get_ID());
	  
	  if (gamma_IDs.size() != 4)
	    _row->shares_photons = 1;
	  else
	    _row->shares_photons = 0;
	
	  _row->pi01_chisq = pi01_mdst.chisq();
	  _row->pi02_chisq = pi02_mdst.chisq();
	  _row->pi01_m     = pi01_mdst.mass();
	  _row->pi02_m     = pi02_mdst.mass();

	  HepLorentzVector pi01(pi01_mdst.px(), pi01_mdst.py(), pi01_mdst.pz(), pi01_mdst.energy());
	  HepLorentzVector pi02(pi02_mdst.px(), pi02_mdst.py(), pi02_mdst.pz(), pi02_mdst.energy());

	  _row->pi01_px = pi01.px();
	  _row->pi01_py = pi01.py();
	  _row->pi01_pz = pi01.pz();
	  _row->pi01_e  = pi01.e();

	  _row->pi02_px = pi02.px();
	  _row->pi02_py = pi02.py();
	  _row->pi02_pz = pi02.pz();
	  _row->pi02_e  = pi02.e();
	
	  _row->mm_off_pi0pi0 = (initial - pi01 - pi02).m();

	  _row->m2_off_pi01 = (initial - pi01).m2();
	  _row->m2_off_pi02 = (initial - pi02).m2();
	  _row->m_pi0pi0 = (pi01 + pi02).m();
	
	  HepLorentzVector mumupi0pi0 = pi01 + pi02 + mup + mum;
	  _row->m_mumupi0pi0          = mumupi0pi0.m();

	  double m2bc_mumupi0pi0 = 4*pow(_row->beam_e, 2) 
	    - pow(mumupi0pi0.px() - initial.px(), 2) 
	    - pow(mumupi0pi0.py() - initial.py(), 2)
	    - pow(mumupi0pi0.pz() - initial.pz(), 2);
	  
	  _row->mbc_mumupi0pi0 = sqrt(fabs(m2bc_mumupi0pi0));

	  if (m2bc_mumupi0pi0 < 0.0) 
	    _row->mbc_mumupi0pi0 *= -1; 	  	   

	  Mdst_gamma pi01_g1_mdst = pi01_mdst.gamma(0);
	  Mdst_gamma pi01_g2_mdst = pi01_mdst.gamma(1);
	  
	  if (pi01_g1_mdst.ecl().energy() < pi01_g1_mdst.ecl().energy()) {
	    Mdst_gamma tmp = pi01_g1_mdst;
	    pi01_g1_mdst = pi01_g2_mdst;
	    pi01_g2_mdst = tmp;
	  }

	  Hep3Vector pi01_g1(pi01_g1_mdst.px(), 
			     pi01_g1_mdst.py(), 
			     pi01_g1_mdst.pz());

	  Hep3Vector pi01_g2(pi01_g2_mdst.px(), 
			     pi01_g2_mdst.py(), 
			     pi01_g2_mdst.pz());
	  
	  _row->pi01_g1theta_lab = pi01_g1.theta() * RADS_TO_DEG;
	  _row->pi01_g2theta_lab = pi01_g2.theta() * RADS_TO_DEG;

	  _row->pi01_g1e = pi01_g1_mdst.ecl().energy();
	  _row->pi01_g2e = pi01_g2_mdst.ecl().energy();
	
	  Mdst_ecl_aux& pi01_g1_eclaux = m_ecl_aux(pi01_g1_mdst.ecl().get_ID());
	  Mdst_ecl_aux& pi01_g2_eclaux = m_ecl_aux(pi01_g2_mdst.ecl().get_ID());

	  _row->pi01_g1e9oe25 = pi01_g1_eclaux.e9oe25();
	  _row->pi01_g2e9oe25 = pi01_g2_eclaux.e9oe25();

	  Mdst_gamma pi02_g1_mdst = pi02_mdst.gamma(0);
	  Mdst_gamma pi02_g2_mdst = pi02_mdst.gamma(1);

	  if (pi02_g1_mdst.ecl().energy() < pi02_g1_mdst.ecl().energy()) {
	    Mdst_gamma tmp = pi02_g1_mdst;
	    pi02_g1_mdst = pi02_g2_mdst;
	    pi02_g2_mdst = tmp;
	  }
	  	  
	  _row->gggg_idhash = gggg_idhash(pi01_g1_mdst.get_ID(), pi01_g2_mdst.get_ID(),
					  pi02_g1_mdst.get_ID(), pi02_g2_mdst.get_ID());

	  if (_row->gggg_idhash != 4009005012)
	    continue;


	  Hep3Vector pi02_g1(pi02_g1_mdst.px(), 
			     pi02_g1_mdst.py(), 
			     pi02_g1_mdst.pz());

	  Hep3Vector pi02_g2(pi02_g2_mdst.px(), 
			     pi02_g2_mdst.py(), 
			     pi02_g2_mdst.pz());
	  
	  _row->pi02_g1e = pi02_g1_mdst.ecl().energy();
	  _row->pi02_g2e = pi02_g2_mdst.ecl().energy();

	  _row->pi02_g1theta_lab = pi02_g1.theta() * RADS_TO_DEG;
	  _row->pi02_g2theta_lab = pi02_g2.theta() * RADS_TO_DEG;
	  
	  Mdst_ecl_aux& pi02_g1_eclaux = m_ecl_aux(pi02_g1_mdst.ecl().get_ID());
	  Mdst_ecl_aux& pi02_g2_eclaux = m_ecl_aux(pi02_g2_mdst.ecl().get_ID());

	  _row->pi02_g1e9oe25 = pi02_g1_eclaux.e9oe25();
	  _row->pi02_g2e9oe25 = pi02_g2_eclaux.e9oe25();

	  cout << "CRAIGB: " << pi02_g1_mdst.get_ID() 
	       << "\t" << m_ecl_aux(pi02_g1_mdst.ecl().get_ID()).e9oe25() 
	       << "\t" << _row->pi02_g1e9oe25 
	       << endl;

	  
	  HepLorentzVector msng = initial - pi01 - pi02 - mup - mum;

	  _row->missing_e  = msng.e();
	  _row->missing_px = msng.px();
	  _row->missing_py = msng.py();
	  _row->missing_pz = msng.pz();
	    
	  mup.boost(-boost_cm);
	  mum.boost(-boost_cm);
	  pi01.boost(-boost_cm);
	  pi02.boost(-boost_cm);

	  _row->p_total_cm = (pi01 + pi02 + mup + mum).vect().mag();
	  
	  _nt->append(*_row);

	  _combos_found++;       

	}
     
  }
  
  //----------------------------------------------------------------------------
  // end_run() handles any bookkeeping necessary at the end of an experimental
  // run--in this case it's empty.
  //----------------------------------------------------------------------------    
  void mumupi0pi0_h5::end_run(BelleEvent* be, int* status) { 
    // Yessir
  }

  //----------------------------------------------------------------------------
  // term() is used to write out any output waiting to be flushed
  //----------------------------------------------------------------------------    
  void mumupi0pi0_h5::term() {
    cout << "CRAIGB : " << _combos_found << " mu+mu-pi0pi0 combos found" << endl;    
    
    // End of analysis, close ntuples
  }


  //----------------------------------------------------------------------------
  // disp_stat() would display some statistics about the events I saved or threw
  // away if I was motivated enough to keep track of such things.
  //----------------------------------------------------------------------------    
  void mumupi0pi0_h5::disp_stat(const char* str) {
    // Who knows
  }


  float mumupi0pi0_h5::two_track_mass(Mdst_charged& t1, Mdst_charged& t2) {
    HepLorentzVector v1 = charged_track_p4(t1, 1);
    HepLorentzVector v2 = charged_track_p4(t2, 1);

    return (v1+v2).m();
  }
    
               
  //----------------------------------------------------------------------------
  // calc_R2() is a function that calculates a physics quantity (normalized Fox-
  // Wolfram second moment, if you're curious).
  //----------------------------------------------------------------------------    
  float mumupi0pi0_h5::calc_R2() {
    Mdst_charged_Manager& m_charged = Mdst_charged_Manager::get_manager();
    Mdst_gamma_Manager& m_gamma = Mdst_gamma_Manager::get_manager();
    
    
    int n_trk = m_charged.count() + m_gamma.count();
    float H0, R1, R2, R3, R4;

    float p_trk[n_trk*3];

    float beam_e = BeamEnergy::E_beam_corr();

    int trkno = 0;
    for (Mdst_charged_Manager::iterator it = m_charged.begin(); 
	 it != m_charged.end(); it++, trkno++)
      {
	p_trk[3*trkno]     = it->px();
	p_trk[3*trkno + 1] = it->py();
	p_trk[3*trkno + 2] = it->pz();
      }

    for (Mdst_gamma_Manager::iterator it = m_gamma.begin();
	 it != m_gamma.end(); it++, trkno++)
      {
	p_trk[3*trkno]     = it->px();
	p_trk[3*trkno + 1] = it->py();
	p_trk[3*trkno + 2] = it->pz();
      }	

    fwjet2(n_trk, p_trk, beam_e, &H0, &R1, &R2, &R3, &R4);

    return R2; 
  }


  HepLorentzVector mumupi0pi0_h5::charged_track_p4(Mdst_charged& trk, int type) {
    Mdst_trk_fit& trkFit = trk.trk().mhyp(type); 
	  
    HepVector a(5,0);
    a[0] = trkFit.helix(0);
    a[1] = trkFit.helix(1);
    a[2] = trkFit.helix(2);
    a[3] = trkFit.helix(3);
    a[4] = trkFit.helix(4);
    
    HepPoint3D pivot(trkFit.pivot(0), trkFit.pivot(1), trkFit.pivot(2));
    Helix ltrk(pivot, a);
    //	  ltrk.pivot(HepPoint3D(0.,0.,0.));
    ltrk.pivot(IP);
    
    return  ltrk.momentum(0., CHARGED_MASS[type]);
  }

    

#ifdef BELLE_NAMESPACE
}
#endif
